#include "StdAfx.h"
#include "GetColorSimilarity.h"
#include "math.h"

LOG4CPLUS_CLASS_IMPLEMENT(GetColorSimilarity, _logger, "FindBuoy")

GetColorSimilarity::~GetColorSimilarity()
{
	LOG4CPLUS_DEBUG(_logger,"GetColorSimilarity::~GetColorSimilarity()");
	if(m_pSimilarityInt!=NULL)
	{
		delete []m_pSimilarityInt;
	}
	m_pSimilarityInt=NULL;

	if(m_pSimilarityDouble!=NULL)
	{
		delete []m_pSimilarityDouble;
	}
	m_pSimilarityDouble=NULL;

	m_closestPoints.clear();
}

void GetColorSimilarity::Run()
{
	LOG4CPLUS_DEBUG(_logger,"GetColorSimilarity::Run()");
	if(m_pSimilarityInt!=NULL)
	{
		delete []m_pSimilarityInt;
	}
	m_pSimilarityInt=new int[m_width*m_height];

	if(m_closestPoints.size()>0)
	{
		m_closestPoints.clear();
		LOG4CPLUS_DEBUG(_logger,"GetColorSimilarity::Run() m_closestPoints.clear()");
	}

	int iDefaultValue;
	{
		int i=600,j=500;
		iDefaultValue=
			 abs(m_pBmpValue[(i*m_width+j)*4]*3-(*(m_ppSampleBmpValue[0]))[(i*m_width+j)*4]-(*(m_ppSampleBmpValue[1]))[(i*m_width+j)*4]-(*(m_ppSampleBmpValue[2]))[(i*m_width+j)*4])
			+abs(m_pBmpValue[(i*m_width+j)*4+1]*3-(*(m_ppSampleBmpValue[0]))[(i*m_width+j)*4+1]-(*(m_ppSampleBmpValue[1]))[(i*m_width+j)*4+1]-(*(m_ppSampleBmpValue[2]))[(i*m_width+j)*4+1])
			+abs(m_pBmpValue[(i*m_width+j)*4+2]*3-(*(m_ppSampleBmpValue[0]))[(i*m_width+j)*4+2]-(*(m_ppSampleBmpValue[1]))[(i*m_width+j)*4+2]-(*(m_ppSampleBmpValue[2]))[(i*m_width+j)*4+2]);
	}
	LOG4CPLUS_DEBUG(_logger,"GetColorSimilarity::Run() iDefaultValue="<<iDefaultValue);
	for(int i=0;i<m_height;++i)
	{
		for(int j=0;j<m_width;++j)
		{
			if(j<m_iAreaLeft||j>m_iAreaLeft+m_iAreaWidth||i<m_iAreaTop||i>m_iAreaTop+m_iAreaHeight)
			{
				m_pSimilarityInt[i*m_width+j]=iDefaultValue;
				continue;
			}
			m_pSimilarityInt[i*m_width+j]=
				 abs(m_pBmpValue[(i*m_width+j)*4]*3 - (*(m_ppSampleBmpValue[0]))[(i*m_width+j)*4] - (*(m_ppSampleBmpValue[1]))[(i*m_width+j)*4] - (*(m_ppSampleBmpValue[2]))[(i*m_width+j)*4])
				+abs(m_pBmpValue[(i*m_width+j)*4+1]*3-(*(m_ppSampleBmpValue[0]))[(i*m_width+j)*4+1]-(*(m_ppSampleBmpValue[1]))[(i*m_width+j)*4+1]-(*(m_ppSampleBmpValue[2]))[(i*m_width+j)*4+1])
				+abs(m_pBmpValue[(i*m_width+j)*4+2]*3-(*(m_ppSampleBmpValue[0]))[(i*m_width+j)*4+2]-(*(m_ppSampleBmpValue[1]))[(i*m_width+j)*4+2]-(*(m_ppSampleBmpValue[2]))[(i*m_width+j)*4+2]);
		}
	}
	TCHAR sFileName[MAX_PATH]="C:\\(";
	//_itot(m_iColorA,sFileName+4,16);
	_tcscat(sFileName,_T(")Similarity.bmp"));
	LOG4CPLUS_DEBUG(_logger, "begin m_pMethods->NormalizedShow,m_width="<<m_width
		<<",m_height="<<m_height<<",sFileName="<<sFileName);
	
#ifdef RECORD_LOG
	m_pMethods->NormalizedShow(m_pSimilarityInt,sFileName,m_width,m_height);
#endif
	GetClosestPointsInRegions();
	LOG4CPLUS_DEBUG(_logger,"GetColorSimilarity::Run() return");
}

void GetColorSimilarity::Run2()
{/*
	LOG4CPLUS_DEBUG(_logger,"GetColorSimilarity::Run2()");
	if(m_pSimilarityDouble!=NULL)
	{
		delete []m_pSimilarityDouble;
	}
	m_pSimilarityDouble=new double[m_width*m_height];
	
	int iColorR = (m_iColor&0xff0000)>>16;
	int iColorG = (m_iColor&0xff00)>>8;
	int iColorB = (m_iColor&0xff);
	LOG4CPLUS_DEBUG(_logger,"GetColorSimilarity::Run2() iColorR="<<iColorR<<",iColorG="<<iColorG<<",iColorB="<<iColorB);
	for(int i=0;i<m_height;++i)
	{
		for(int j=0;j<m_width;++j)
		{
			m_pSimilarityDouble[i*m_width+j]=(m_pBmpValue[(i*m_width+j)*4]-iColorB)*(m_pBmpValue[(i*m_width+j)*4]-iColorB)
				+(m_pBmpValue[(i*m_width+j)*4+1]-iColorG)*(m_pBmpValue[(i*m_width+j)*4+1]-iColorG)
				+(m_pBmpValue[(i*m_width+j)*4+2]-iColorR)*(m_pBmpValue[(i*m_width+j)*4+2]-iColorR);
			m_pSimilarityDouble[i*m_width+j]=sqrt(m_pSimilarityDouble[i*m_width+j]);
		}
	}
	TCHAR sFileName[1024]="C:\\(";
	_itot(m_iColor,sFileName+4,16);
	_tcscat(sFileName,_T(")Similarity.bmp"));
	LOG4CPLUS_DEBUG(_logger, "begin m_pMethods->NormalizedShow,m_width="<<m_width
		<<",m_height="<<m_height<<",sFileName="<<sFileName);
	
	m_pMethods->NormalizedShow(m_pSimilarityDouble,sFileName,m_width,m_height);
	LOG4CPLUS_DEBUG(_logger,"GetColorSimilarity::Run2() return");*/
}

void GetColorSimilarity::GetClosestPointsInRegions()
{
	int iRegionNumx=m_iAreaWidth/m_iRegionLength;
	int iRegionNumy=m_iAreaHeight/m_iRegionLength;
	m_closestPoints.resize(iRegionNumx*iRegionNumy);
	LOG4CPLUS_DEBUG(_logger, "GetColorSimilarity::GetClosestPointsInRegions() m_closestPoints.size()="
		<<m_closestPoints.size()<<",iNewSize="<<iRegionNumx*iRegionNumy);
	for(int jj=0;jj<iRegionNumy;++jj)
	{
		for(int ii=0;ii<iRegionNumx;++ii)
		{
			m_closestPoints[jj*iRegionNumx+ii].iSimilarity=m_pSimilarityInt[(jj*m_iRegionLength+m_iAreaTop)*m_width+ii*m_iRegionLength+m_iAreaLeft];
			for(int j=0;j<m_iRegionLength;++j)
			{
				for(int i=0;i<m_iRegionLength;++i)
				{
					int postionx=ii*m_iRegionLength+i+m_iAreaLeft;
					int postiony=jj*m_iRegionLength+j+m_iAreaTop;
					int iSimilarityInt=m_pSimilarityInt[postiony*m_width+postionx];
					if(m_closestPoints[jj*iRegionNumx+ii].iSimilarity<=iSimilarityInt)
					{
						m_closestPoints[jj*iRegionNumx+ii].iSimilarity=iSimilarityInt;
						m_closestPoints[jj*iRegionNumx+ii].x=postionx;
						m_closestPoints[jj*iRegionNumx+ii].y=postiony;
						//closestPoints[jj*iRegionNumx+ii].iRegionx=ii;
						//closestPoints[jj*iRegionNumx+ii].iRegiony=jj;
					}	
				}
			}
			LOG4CPLUS_DEBUG(_logger,"GetColorSimilarity::GetClosestPointsInRegions() Regionx="<<
				ii<<" Regiony="<<jj<<" x="<<m_closestPoints[jj*iRegionNumx+ii].x<<
				" y="<<m_closestPoints[jj*iRegionNumx+ii].y<<" iSimilarity="<<m_closestPoints[jj*iRegionNumx+ii].iSimilarity);
		}
	}
}

int GetColorSimilarity::IsUnacceptableAngle(int x,int y)
{
	double cosa=m_pMethods->GetBlockDirectionCosa(x,y,20);
	/*LOG4CPLUS_DEBUG(_logger,"GetColorSimilarity::IsUnacceptableAngle() Point("
		<<x<<","<<y<<")"<<" cosa="<<cosa);*/
	if(cosa>0&&cosa<1)
	{
		return 0;
	}
	else
	{
		return -1;
	}

}

PointInfo GetColorSimilarity::GetClosestPoint()
{
	//sort closestPoints by iSimilarity & find the first point which's angle is around 45 degree. 
	//LOG4CPLUS_DEBUG(_logger,"GetColorSimilarity::GetClosestPoint() begin ");
	LOG4CPLUS_DEBUG(_logger,"GetColorSimilarity::GetClosestPoint() m_closestPoints.size()="<<m_closestPoints.size());
	PointInfo rlt;
	for(int i=0;i<m_closestPoints.size();++i)
	{
		int maxIndex=0;
		
		for(int j=1;j<m_closestPoints.size()-i;++j)
		{
			if(m_closestPoints[maxIndex].iSimilarity<m_closestPoints[j].iSimilarity)
			{
				maxIndex=j;
			}
		}
		if(maxIndex!=m_closestPoints.size()-i-1)
		{
			PointInfo tPointInfo=m_closestPoints[maxIndex];
			m_closestPoints[maxIndex]=m_closestPoints[m_closestPoints.size()-i-1];
			m_closestPoints[m_closestPoints.size()-i-1]=tPointInfo;	
		}
		LOG4CPLUS_DEBUG(_logger,"GetColorSimilarity::GetClosestPointsInRegions() test point ("<<m_closestPoints[m_closestPoints.size()-i-1].x<<","
			<<m_closestPoints[m_closestPoints.size()-i-1].y<<") iSimilarity="<<m_closestPoints[m_closestPoints.size()-i-1].iSimilarity);
		//if(!IsUnacceptableAngle(m_closestPoints[m_closestPoints.size()-i-1].x,m_closestPoints[m_closestPoints.size()-i-1].y))
		{
			LOG4CPLUS_DEBUG(_logger,"GetColorSimilarity::GetClosestPointsInRegions() find point ("<<m_closestPoints[m_closestPoints.size()-i-1].x<<","<<m_closestPoints[m_closestPoints.size()-i-1].y<<").");
			rlt=m_closestPoints[m_closestPoints.size()-i-1];
			m_closestPoints.resize(m_closestPoints.size()-i-1);
			return rlt;
		}		
	}

	rlt.x=-1;
	rlt.y=-1;
	rlt.iSimilarity=-1;
	m_closestPoints.clear();
	return rlt;
}

void GetColorSimilarity::Init(MethodContainer *pMet,int iLeft,int iTop,int iWidth,int iHeight,int iRegionLength)
{
	Pretreatment::Init(pMet);
	
//	m_iRegionLength=iRegionLength;
	m_iAreaTop=iTop;
	m_iAreaLeft=iLeft;
	m_iAreaWidth=iWidth;
	m_iAreaHeight=iHeight;
	m_iRegionLength=iRegionLength;

	if(m_pSimilarityInt!=NULL)
	{
		delete []m_pSimilarityInt;
	}
	m_pSimilarityInt=NULL;
	
	if(m_pSimilarityDouble!=NULL)
	{
		delete []m_pSimilarityDouble;
	}
	m_pSimilarityDouble=NULL;
	
	m_isUninited=1;
	LOG4CPLUS_DEBUG(_logger,"GetColorSimilarity::Init() m_width="<<m_width<<",m_height="<<m_height);
}

long GetColorSimilarity::SetSample(vector<BYTE>* pSampleBmpValue,int iImgId)
{
	m_ppSampleBmpValue[iImgId]=pSampleBmpValue;
	if(m_isUninited > 0)
	{
		m_isUninited=iImgId+2;
	}
	else if( 0 == m_isUninited)
	{
		m_isUninited=2;
	}
	else
	{
		return -1;
	}

	if(4==m_isUninited)
	{
		m_isUninited=0;
		Run();
	}
		
	return 1;	
}

